package at.jku.pervasive.recognition;

import javax.vecmath.Point3d;
import javax.vecmath.Vector2d;

import at.jku.pervasive.Position;

public class AngleGestureRecognizer implements GestureRecognizer
{

	/**
	 * Calculate the XY, XZ and YZ angles in degrees between the given points.
	 * @param leftPoint
	 * @param refPoint
	 * @param rightPoint
	 * @return array, containing the angles, where arr[0] => XY angle, arr[1] =>
	 *         XZ angle and arr[2] => YZ angle
	 */
	private double[] calculatePlaneAngles(Point3d leftPoint, Point3d refPoint, Point3d rightPoint)
	{
		// get angle in XY
		Vector2d refXY = new Vector2d(refPoint.x, refPoint.y);
		Vector2d leftXY = new Vector2d(leftPoint.x, leftPoint.y);
		leftXY.sub(refXY);
		Vector2d rightXY = new Vector2d(rightPoint.x, rightPoint.y);
		rightXY.sub(refXY);
		double angleXY = leftXY.angle(rightXY);
		double angleXYDegrees = Math.toDegrees(angleXY);

		// get angle in XZ
		Vector2d refXZ = new Vector2d(refPoint.x, refPoint.z);
		Vector2d leftXZ = new Vector2d(leftPoint.x, leftPoint.z);
		leftXZ.sub(refXZ);
		Vector2d rightXZ = new Vector2d(rightPoint.x, rightPoint.z);
		rightXZ.sub(refXZ);

		double angleXZ = leftXZ.angle(rightXZ);
		double angleXZDegrees = Math.toDegrees(angleXZ);

		// get angle in YZ
		Vector2d refYZ = new Vector2d(refPoint.y, refPoint.z);
		Vector2d leftYZ = new Vector2d(leftPoint.y, leftPoint.z);
		leftYZ.sub(refYZ);
		Vector2d rightYZ = new Vector2d(rightPoint.y, rightPoint.z);
		rightYZ.sub(refYZ);
		double angleYZ = leftYZ.angle(rightYZ);
		double angleYZDegrees = Math.toDegrees(angleYZ);

		// finally return the array with the calculates angles
		return new double[] { angleXYDegrees, angleXZDegrees, angleYZDegrees };
	}

	protected Point3d getPoint3d(Position position)
	{
		Point3d point3d = new Point3d();
		point3d.x = position.getX();
		point3d.y = position.getY();
		point3d.z = position.getZ();
		return point3d;
	}

	@Override
	public Gesture getRecognizedGesture(Position left, Position reference, Position right)
	{
		GestureCharacteristics gC = new GestureCharacteristics(left, reference, right);

		// extract gestures
		if (gC.leftIsLowerThanRight() && gC.refIsLowerThanRight())
		{
			if (gC.refHasEqualHeightToLeft())
				return Gesture.LSRU;
			else if (gC.leftIsLowerThanRef())
				return Gesture.LDRU;
		}
		else if (gC.rightIsLowerThanLeft() && gC.refIsLowerThanLeft())
		{
			if (gC.refHasEqualHeightToRight())
				return Gesture.LURS;
			else if (gC.rightIsLowerThanRef())
				return Gesture.LURD;
		}
		else if (gC.leftHasEqualHeightToRight() && gC.refHasEqualHeightToRight())
		{
			if (gC.angleIs180())
				return Gesture.LSRS180;
			else if (gC.angleIs90())
				return Gesture.LSRS90;
			else if (gC.angleIs0())
				return Gesture.LSRS0;
		}
		else if (gC.rightIsLowerThanRef() && gC.refHasEqualHeightToLeft())
		{
			return Gesture.LSRD;
		}
		else if (gC.leftIsLowerThanRef() && gC.refHasEqualHeightToRight())
		{
			return Gesture.LDRS;
		}
		else if (gC.refIsLowerThanLeft() && gC.refIsLowerThanRight() && gC.leftHasEqualHeightToRight())
		{
			return Gesture.LURU;
		}
		else if (gC.leftIsLowerThanRef() && gC.rightIsLowerThanRef())
		{
			return Gesture.LDRD;
		}

		return Gesture.NONE;
	}

}
